import numpy as np
import cv2
import math
from matplotlib import pyplot as plt
import time
import serial

ser = serial.Serial('COM3', 9600, timeout=0.5)

cap = cv2.VideoCapture(1)

while(1):
            
    #色调还原度高，适合定位小人
    cap.set(cv2.CAP_PROP_EXPOSURE,-4.0)
    cap.set(cv2.CAP_PROP_BRIGHTNESS,124.0)
    cap.set(cv2.CAP_PROP_CONTRAST,63.0)
    cap.set(cv2.CAP_PROP_SATURATION,128.0)
    cap.set(cv2.CAP_PROP_HUE,90.0)
    
    ret,frame1=cap.read()
    if ret:
        cv2.imshow('capture1',frame1) 
        a=int(frame1.shape[0]/3)
        b=int(frame1.shape[1]/3)
        roi1=frame1[a:2*a,b:2*b,]
        
        #1.角点=>矩形中心
        edges=cv2.Canny(roi1,100,200)
        corners=cv2.goodFeaturesToTrack(edges,100,0.1,50)
        corners = np.int0(corners)
        c=700
        point1=(0,0)
        for corner in corners:
            x,y=corner.ravel()
            cv2.circle(roi1,(x,y),3,(0,0,255),-1)
            if y<c:
                c=y
                point2=point1
                point1=(x,y)
        x1,y1=point1
        x2,y2=point2
        x,y=x1,y2
        rcenter=(x,y)
        cv2.circle(roi1,(x,y),6,(0,255,0),-1)
        cv2.circle(roi1,(x1,y1),4,(255,0,0),-1)
        cv2.circle(roi1,(x2,y2),4,(255,0,0),-1)        
        cv2.imshow('roi1',roi1)        
        #cv2.imshow('edges',edges)
        
        #2.用HoughCircles函数确定圆的坐标   
        gray_img = cv2.cvtColor(roi1, cv2.COLOR_BGR2GRAY)   # 灰度处理        
        img = cv2.medianBlur(gray_img, 5)   # medianBlur 平滑（模糊）处理                       
        circles = cv2.HoughCircles(img, cv2.HOUGH_GRADIENT, 1, 100, param1=100, param2=5, minRadius=40, maxRadius=60)    # 圆检测        
        circles = np.uint16(np.around(circles))     # 转化整数         
        c=700                
        for i in circles[0,:]:           
            cv2.circle(roi1,(i[0],i[1]),i[2],(0,255,0),2)   # 勾画圆形，roi1图像、(i[0],i[1])圆心坐标，i[2]是半径            
            cv2.circle(roi1,(i[0],i[1]),2,(0,0,255),3)   # 勾画圆心，圆心实质也是一个半径为2的圆形
            if i[1]<c:
                c=i[1]
                x=i[0]
                y1=i[1]
                radius=i[2]            
        y=int(y1+radius/2)
        ccenter=(x,y)
        cv2.circle(roi1,ccenter,6,(255,0,0),-1)
        # 显示图像
        cv2.imshow("HoughCirlces", roi1)
        
    #图片对比度高，适合得到边缘
    cap.set(cv2.CAP_PROP_EXPOSURE,-6.0)
    cap.set(cv2.CAP_PROP_BRIGHTNESS,128.0)
    cap.set(cv2.CAP_PROP_CONTRAST,32.0)
    cap.set(cv2.CAP_PROP_SATURATION,64.0)
    cap.set(cv2.CAP_PROP_HUE,0.0)  
    
    ret,frame2=cap.read()     
    if ret:
        cv2.imshow('capture2',frame2) 
        a=int(frame2.shape[0]/3)
        b=int(frame2.shape[1]/3)
        roi2=frame2[a:2*a,b:2*b,]
        
        #3.找到小人底部
        hsv=cv2.cvtColor(roi2,cv2.COLOR_BGR2HSV)
        lower_blue = np.array([115,75,75])    #设定蓝色的阈值
        upper_blue = np.array([130,255,125])
        mask=cv2.inRange(hsv,lower_blue,upper_blue)
        res=cv2.bitwise_and(roi2, roi2,mask=mask)                
        for y in range(0,len(mask)):
            for x in range(0,len(mask[y])):
                if mask[y][x]==255:
                    people=(x,y)    
        #print(people)            
        cv2.circle(roi2,people,6,(0,255,0),-1)
        cv2.imshow('roi2',roi2)
        #cv2.imshow('hsv',hsv)
        #cv2.imshow('maskq',mask)
        #cv2.imshow('resq',res)
            
        
    #4.得到距离        
    cv2.circle(roi2,rcenter,6,(0,0,255),-1)
    cv2.line(roi2,people,rcenter,(255,0,0),1)
    cv2.imshow('final',roi2)
    x1,y1=rcenter
    x2,y2=people
    d=math.sqrt((x2-x1)**2+(y2-y1)**2)
    p=int(d/17)
    print(d,p)
    
    #5.与Arduino通信
    if p>=10:
        p=9
    q=str(p)
    s=ser.readline()
    while s==str().encode('utf-8'):
        ser.write(q.encode('utf-8'))
        time.sleep(0.1)
        s=ser.readline()
    time.sleep(1)
    print(s)                    
    
    if cv2.waitKey(10) & 0xFF == ord('q'):
         break 
     
cap.release()
cv2.destroyAllWindows()
ser.close()